from bzrc import BZRC, Command
from PriorityQueue import PriorityQueue
import sys, math, time, optparse, random, pprint

PENALIZE = True
ALONG_COST = 1.5 * 10
TO_COST = 1.3 * 10
FROM_COST = 1.1 * 10
NORMAL_COST = 1
MAX_COST = 15
PEN_DIST = MAX_COST - 1		

class DFS:
	def __init__(self, nodes, curX, curY, flagX, flagY):
		self.nodes = nodes
		self.startPos = (curX, curY)
		self.flagPos = (flagX,flagY)
		self.lastNode = (curX, curY)
		self.stack = [(self.startPos[0], self.startPos[1], (None, None))]
		self.path = []
		self.searched = {}
		
	def search(self):
		print "Running search..."
		exists = self.iter_search()
		if not exists:
			print "Error! Could not find path from ", self.startPos, " to ", self.flagPos
		lastNode = self.path.pop() # We can assume the final node was our goal
		pos = (lastNode[0], lastNode[1])
		finalPath = [pos]
		allSearched = [pos]
		parent = lastNode[2]
		while self.path:
			node = self.path.pop()
			pos = (node[0], node[1])
			if pos == (None, None):
				continue
			allSearched.append(pos)
			if pos != parent:
				continue
			parent = node[2]
			finalPath.insert(0, pos)
		print "Done!"
		print "Total nodes popped: ", len(allSearched)
		print "Length of final path: ", len(finalPath)
		return finalPath, allSearched
		
	def iter_search(self):
		while self.stack:
			if self.iter_step():
				return True
		return False
		
	def iter_step(self):
		node = self.stack.pop()
		pos = (node[0], node[1])
		self.lastNode = pos
		if not pos in self.nodes or self.nodes[pos] == 0 or pos in self.searched:
			return False # Discard this node
		self.searched[pos] = 1
		self.path.append(node)
		if pos == self.flagPos:
			return True
		self.stack.append((pos[0]+1, pos[1]-1, pos))
		self.stack.append((pos[0]-1, pos[1]-1, pos))
		self.stack.append((pos[0]+1, pos[1]+1, pos))
		self.stack.append((pos[0]-1, pos[1]+1, pos))
		self.stack.append((pos[0], pos[1]-1, pos))
		self.stack.append((pos[0], pos[1]+1, pos))
		self.stack.append((pos[0]+1, pos[1], pos))
		self.stack.append((pos[0]-1, pos[1], pos))
		return False

	def step(self, steps):
		toReturn = []
		for i in range(steps):
			if not self.stack:
				break
			done = self.iter_step()
			toReturn += self.stack[-8:]
			if done:
				break
		return toReturn
		

	def clear(self):
		self.path = []
		self.stack = [(self.startPos[0], self.startPos[1], (None, None))]
		self.searched = {}
		
class BFS:
	def __init__(self, nodes, curX, curY, flagX, flagY):
		self.nodes = nodes
		self.startPos = (curX, curY)
		self.flagPos = (flagX,flagY)
		self.lastNode = (curX, curY)
		self.queue = [(self.startPos[0], self.startPos[1], (None, None))]
		self.path = []
		self.searched = {}
		
	def search(self):
		print "Running search..."
		exists = self.iter_search()
		if not exists:
			print "Error! Could not find path from ", self.startPos, " to ", self.flagPos
		lastNode = self.path.pop() # We can assume the final node was our goal
		pos = (lastNode[0], lastNode[1])
		finalPath = [pos]
		allSearched = [pos]
		parent = lastNode[2]
		while self.path:
			node = self.path.pop()
			pos = (node[0], node[1])
			if pos == (None, None):
				continue
			allSearched.append(pos)
			if pos != parent:
				continue
			parent = node[2]
			finalPath.insert(0, pos)
		print "Done!"
		print "Total nodes popped: ", len(allSearched)
		print "Length of final path: ", len(finalPath)
		return finalPath, allSearched
		
	def iter_search(self):
		while self.queue:
			if self.iter_step():
				return True
		return False
		
	def iter_step(self):
		node = self.queue.pop(0)
		pos = (node[0], node[1])
		self.lastNode = pos
		if not pos in self.nodes or self.nodes[pos] == 0 or pos in self.searched:
			return False # Discard this node
		self.searched[pos] = 1
		self.path.append(node)
		if pos == self.flagPos:
			return True
		self.queue.append((pos[0]+1, pos[1]-1, pos))
		self.queue.append((pos[0]-1, pos[1]-1, pos))
		self.queue.append((pos[0]+1, pos[1]+1, pos))
		self.queue.append((pos[0]-1, pos[1]+1, pos))
		self.queue.append((pos[0], pos[1]-1, pos))
		self.queue.append((pos[0], pos[1]+1, pos))
		self.queue.append((pos[0]+1, pos[1], pos))
		self.queue.append((pos[0]-1, pos[1], pos))
		return False

	def step(self, steps):
		toReturn = []
		for i in range(steps):
			if not self.queue:
				break
			done = self.iter_step()
			toReturn += self.queue[-8:]
			if done:
				break
		return toReturn
		

	def clear(self):
		self.path = []
		self.queue = [(self.startPos[0], self.startPos[1], (None, None))]
		self.searched = {}
		
class AStar:
	def __init__(self, bzrc, curX, curY, size):
		nodes, goal, penalties = parse_grid(size, bzrc)
		self.nodes = nodes
		self.startPos = (curX, curY)
		self.flagPos = (goal[0],goal[1])
		self.lastNode = (curX, curY)
		self.frontier = PriorityQueue()
		self.frontier.enqueue(0, (curX, curY), None, 0) 
		self.path = []
		self.searched = {}
		self.newNodes = []
		self.penalties = penalties
		self.bestSeenCost = 10000000000000
		self.bestSeen = (0,0)
		
	def search(self):
		print "Running search..."
		exists = self.iter_search()
		if not exists:
			print "Error! Could not find path from ", self.startPos, " to ", self.flagPos
		allSearched = []
		for node in self.nodes:
			if node in self.searched:
				allSearched.append(node)
				
		finalPath = [exists]
		curNode = exists
		nextNode = self.searched[exists]
		while nextNode != None:
			finalPath.append(nextNode)
			curNode = nextNode
			nextNode = self.searched[curNode]
		print "Total nodes popped: ", len(allSearched)
		print "Length of final path: ", len(finalPath)
		return finalPath, allSearched
		
	def step(self, steps):
		toReturn = []
		for i in range(steps):
			if not self.frontier:
				break
			done = self.iter_step()
			toReturn += self.newNodes
			if done:
				break
		return toReturn
		
	def iter_search(self):
		while self.frontier.has_item():
			answer = self.iter_step()
			if answer:
				return answer
		return False
		
	def iter_step(self):
		priority, cost, curNode, parent = self.frontier.dequeue()
		if not curNode:
			return False
		if not curNode in self.searched:
			self.searched[curNode] = parent
		if (curNode[0], curNode[1]) == self.flagPos:
			return curNode
		self.newNodes = []
		self.check_node((curNode[0]+1, curNode[1]-1), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]-1), cost, curNode)
		self.check_node((curNode[0]+1, curNode[1]+1), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]+1), cost, curNode)
		self.check_node((curNode[0], curNode[1]-1), cost, curNode)
		self.check_node((curNode[0], curNode[1]+1), cost, curNode)
		self.check_node((curNode[0]+1, curNode[1]), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]), cost, curNode)
		return False
		
	def check_node(self, node, cost, parent):
		if node in self.nodes and self.nodes[node] and not node in self.searched:
			newCost = cost + self.calc_cost(node, parent)
			if not self.frontier.get_data_priority(node):
				self.frontier.enqueue(newCost + self.heuristic_cost(node), node, parent, newCost)
				self.newNodes.append(node)
			elif self.frontier.get_data_priority(node) > newCost + self.heuristic_cost(node):
				self.frontier.reprioritize(newCost + self.heuristic_cost(node), node, parent, newCost)
				if node in self.searched:
					self.searched[curNode] = parent
				
	def heuristic_cost(self, node):
		#Our heuristic is just straight-line distance
		dist = math.hypot(node[0] - self.flagPos[0], node[1] - self.flagPos[1])
		if (dist < self.bestSeenCost):
			self.bestSeenCost = dist
			self.bestSeen = node
		return dist
			
	def calc_cost(self, node, parent):
		if PENALIZE:
			return self.penalties[node[0],node[1]] * math.hypot(node[0] - parent[0], node[1] - parent[1])
		else:
			return NORMAL_COST * math.hypot(node[0] - parent[0], node[1] - parent[1])
						
	
	def is_adjacent(self, node):
		if (node[0],node[1]+1) in self.nodes and (self.nodes[(node[0],node[1]+1)] == 0):
			return True
		if (node[0]+1,node[1]+1) in self.nodes and (self.nodes[(node[0]+1,node[1]+1)] == 0):
			return True
		if (node[0]-1,node[1]+1) in self.nodes and (self.nodes[(node[0]-1,node[1]+1)] == 0):
			return True
		if (node[0]-1,node[1]) in self.nodes and (self.nodes[(node[0]-1,node[1])] == 0):
			return True
		if (node[0]+1,node[1]+1) in self.nodes and (self.nodes[(node[0]+1,node[1]+1)] == 0):
			return True
		if (node[0]+1,node[1]-1) in self.nodes and (self.nodes[(node[0]+1,node[1]-1)] == 0):
			return True
		if (node[0],node[1]-1) in self.nodes and (self.nodes[(node[0],node[1]-1)] == 0):
			return True
		if (node[0]-1,node[1]-1) in self.nodes and (self.nodes[(node[0]-1,node[1]-1)] == 0):
			return True
		return False
		
	def clear(self):
		self.path = []
		self.frontier = PriorityQueue()
		self.frontier.enqueue(0, (self.startPos[0], self.startPos[1]), None, 0)
		self.searched = {}

class UCS:
	def __init__(self, nodes, curX, curY, flagX, flagY):
		self.nodes = nodes
		self.startPos = (curX, curY)
		self.flagPos = (flagX,flagY)
		self.lastNode = (curX, curY)
		self.frontier = PriorityQueue()
		self.frontier.enqueue(0, (curX, curY), None, 0) 
		self.path = []
		self.searched = {}
		self.newNodes = []
		
	def search(self):
		print "Running search..."
		exists = self.iter_search()
		if not exists:
			print "Error! Could not find path from ", self.startPos, " to ", self.flagPos
		allSearched = []
		for node in self.nodes:
			if node in self.searched:
				allSearched.append(node)
				
		finalPath = [exists]
		curNode = exists
		nextNode = self.searched[exists]
		while nextNode != None:
			finalPath.append(nextNode)
			curNode = nextNode
			nextNode = self.searched[curNode]
		print "Total nodes popped: ", len(allSearched)
		print "Length of final path: ", len(finalPath)
		return finalPath, allSearched
		
	def step(self, steps):
		toReturn = []
		for i in range(steps):
			if not self.frontier:
				break
			done = self.iter_step()
			toReturn += self.newNodes
			if done:
				break
		return toReturn
		
	def iter_search(self):
		while self.frontier.has_item():
			answer = self.iter_step()
			if answer:
				return answer
		return False
		
	def iter_step(self):
		priority, cost, curNode, parent = self.frontier.dequeue()
		if not curNode in self.searched:
			self.searched[curNode] = parent
		if (curNode[0], curNode[1]) == self.flagPos:
			return curNode
		self.newNodes = []
		self.check_node((curNode[0]+1, curNode[1]-1), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]-1), cost, curNode)
		self.check_node((curNode[0]+1, curNode[1]+1), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]+1), cost, curNode)
		self.check_node((curNode[0], curNode[1]-1), cost, curNode)
		self.check_node((curNode[0], curNode[1]+1), cost, curNode)
		self.check_node((curNode[0]+1, curNode[1]), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]), cost, curNode)
		return False
		
	def check_node(self, node, cost, parent):
		if node in self.nodes and self.nodes[node] and not node in self.searched:
			newCost = cost + self.calc_cost(node, parent)
			if not self.frontier.get_data_priority(node):
				self.frontier.enqueue(newCost, node, parent, newCost)
				self.newNodes.append(node)
			elif self.frontier.get_data_priority(node) > newCost:
				self.frontier.reprioritize(newCost, node, parent, newCost)
				if node in self.searched:
					self.searched[curNode] = parent
			
	def calc_cost(self, node, parent):
		if PENALIZE:
			if self.is_adjacent(node):
				if self.is_adjacent(parent):
					return ALONG_COST * math.hypot(node[0] - parent[0], node[1] - parent[1])
				else:
					return TO_COST * math.hypot(node[0] - parent[0], node[1] - parent[1])
			elif self.is_adjacent(parent):
				return FROM_COST * math.hypot(node[0] - parent[0], node[1] - parent[1])
			return NORMAL_COST * math.hypot(node[0] - parent[0], node[1] - parent[1])
		else:
			return NORMAL_COST * math.hypot(node[0] - parent[0], node[1] - parent[1])
						
	
	def is_adjacent(self, node):
		if (node[0],node[1]+1) in self.nodes and (self.nodes[(node[0],node[1]+1)] == 0):
			return True
		if (node[0]+1,node[1]+1) in self.nodes and (self.nodes[(node[0]+1,node[1]+1)] == 0):
			return True
		if (node[0]-1,node[1]+1) in self.nodes and (self.nodes[(node[0]-1,node[1]+1)] == 0):
			return True
		if (node[0]-1,node[1]) in self.nodes and (self.nodes[(node[0]-1,node[1])] == 0):
			return True
		if (node[0]+1,node[1]+1) in self.nodes and (self.nodes[(node[0]+1,node[1]+1)] == 0):
			return True
		if (node[0]+1,node[1]-1) in self.nodes and (self.nodes[(node[0]+1,node[1]-1)] == 0):
			return True
		if (node[0],node[1]-1) in self.nodes and (self.nodes[(node[0],node[1]-1)] == 0):
			return True
		if (node[0]-1,node[1]-1) in self.nodes and (self.nodes[(node[0]-1,node[1]-1)] == 0):
			return True
		return False
		
	def clear(self):
		self.path = []
		self.frontier = PriorityQueue()
		self.frontier.enqueue(0, (self.startPos[0], self.startPos[1]), None, 0)
		self.searched = {}
			

class IDS:
	def __init__(self, nodes, curX, curY, flagX, flagY):
		self.nodes = nodes
		self.startPos = (curX, curY)
		self.flagPos = (flagX,flagY)
		self.lastNode = (curX, curY)
		self.path = []
		self.searched = {}
		self.depth = 1
		sys.setrecursionlimit(5000) # Just in case
		
	def search(self):
		print "Running search..."
		exists = self.rec_search()
		if not exists: # Impossible for now
			print "Error! Could not find path from ", self.startPos, " to ", self.flagPos
		finalPath = self.path # Conveniently enough, this is our exact path
		allSearched = []
		for pos in self.searched:
			allSearched.append(pos)
		print "Done!"
		print "Total nodes popped: ", len(allSearched)
		print "Length of final path: ", len(finalPath)
		return finalPath, allSearched
	
	def rec_search(self):
		while True: # This assumes there /is/ a solution
			self.path = [self.startPos]
			self.searched = {}
			if self.rec_step(self.startPos, self.depth):
				return True
			self.depth += 1000
			# print "Increasing depth to ", self.depth

	def rec_step(self, pos, depth):
		self.lastNode = pos
		if depth == 0 or not pos in self.nodes or self.nodes[pos] == 0 or (pos in self.searched and self.searched[pos] >= depth): # We can't explore this node any deeper (greater depth == more possibility)
			return False # Discard this node
		self.path.append(pos)
		self.searched[(pos)] = depth
		if pos == self.flagPos:
			return True
		successors = []
		successors.append((pos[0]+1, pos[1]-1))
		successors.append((pos[0]-1, pos[1]-1))
		successors.append((pos[0]+1, pos[1]+1))
		successors.append((pos[0]-1, pos[1]+1))
		successors.append((pos[0], pos[1]-1))
		successors.append((pos[0], pos[1]+1))
		successors.append((pos[0]+1, pos[1]))
		successors.append((pos[0]-1, pos[1]))
		for s in successors:
			if self.rec_step(s, depth-1):
				return True
		self.path.pop() # Guess we weren't The One

	def step(self, steps):
		for i in range(steps):
			self.searched = {}
			done = self.rec_step(self.startPos, self.depth)
			if done:
				break
			self.depth += 1
			# print "Increasing depth to ", self.depth
		toReturn = []
		for pos in self.searched:
			toReturn.append(pos)
		return toReturn
		

	def clear(self):
		self.path = []
		self.stack = [(self.startPos[0], self.startPos[1], (None, None))]
		self.searched = {}

def parse_grid(size, bzrc):
	toReturn = {}
	penaltyGrid = {}
	
	#Get the obstacles
	obstacles = bzrc.get_obstacles()
	otherTanks = bzrc.get_othertanks()
	
	furthestRight = -size/2
	
	for i in range(0, size):
		for j in range(0, size):
			toReturn[(int(i-size/2),int(j-size/2))] = 1
			penaltyGrid[(int(i-size/2),int(j-size/2))] = NORMAL_COST
	
	#Iterate through the obstacles, drawing line in between them
	for obstacle in obstacles:
		for i, point in enumerate(obstacle):
			#figure out which direction to go
			if i+1 < len(obstacle):
				point2 = obstacle[i+1]
			else:
				point2 = obstacle[0]
				
			diffx = point2[0] - point[0]
			diffy = point2[1] - point[1]
			
			rge = {}
			
			if diffx < 0:
				rge = range(0, int(diffx), -1)
			elif diffx > 0:
				rge = range(0, int(diffx), 1)
				
			if rge:
				for x in rge:
					toReturn[int(point[0])+x, int(point[1])] = 0
					for y in range(-PEN_DIST, PEN_DIST, 1):
						penaltyGrid[int(point[0])+x, int(point[1])+y] = MAX_COST - math.fabs(y)						
			else:
				if diffy < 0:
					rge = range(0, int(diffy), -1)
				else:
					rge = range(0, int(diffy), 1)
				for y in rge:
					toReturn[int(point[0]), int(point[1])+y] = 0
					for x in range(-PEN_DIST, PEN_DIST, 1):
						penaltyGrid[int(point[0])+x, int(point[1])+y] = MAX_COST - math.fabs(x)	
					
			if point[0] > furthestRight:
				furthestRight = point[0]
				
	for i in range(int(furthestRight+1), int(size/2)):
		for j in range(0, size):
			toReturn[(int(i),int(j-size/2))] = 0
			
	goal = (int(bzrc.get_flags()[2].x), int(bzrc.get_flags()[2].y))
				
	return toReturn, goal, penaltyGrid

def parse_grid_old(nodes, othertanks, size):
	toReturn = {}
	for i in range(0, size):
		for j in range(0, size):
			if int(nodes[i][j]) == 0:
				toReturn[(int(i-size/2),int(j-size/2))] = 1
			else:
				toReturn[(int(i-size/2),int(j-size/2))] = 0
	for tank in othertanks:
		for i in xrange(-2, 2):
			for j in xrange(-2, 2):
				pos = (int(tank.x+i),int(tank.y+j))
				if pos in toReturn:
					toReturn[pos] = 0
	return toReturn

if __name__ == "__main__":	# Process CLI arguments.
	sys.setrecursionlimit(24000)
	parser = optparse.OptionParser(usage = "%prog [options]",
								   version = "%prog 0.1")
	parser.add_option("-o", "--host", type="string",dest="host",
			metavar="HOST",default='localhost',
			help="hostname for the server")
	parser.add_option("-p", "--port", type="int",dest="port",
			metavar="PORT",default=3907,
			help="port number for the server")
	parser.add_option("-s", "--search", type="string", dest="search",
			metavar="SEARCH",default="DFS",
			help="Desired search to use")

	(options, args) = parser.parse_args()

	# Connect.
	bzrc = BZRC(options.host, int(options.port))

	mytanks = bzrc.get_mytanks()
	mytank = mytanks[0]
	flags = bzrc.get_flags()
	flag = None
	for f in flags:
		if f.color == 'green':
			flag = f
			break
		
	constants = bzrc.get_constants()
	nodes, goal = parse_grid(bzrc.get_othertanks(), int(constants['worldsize']))
	del bzrc
	search = globals()[options.search](nodes, int(mytank.x), int(mytank.y), int(flag.x), int(flag.y))
	path = search.search()
